<!DOCTYPE html>
<html lang="zh-CN">
<head>
  <meta charset="UTF-8" />
  <meta name="viewport" content="width=device-width, initial-scale=1" />
  <title>ROS2 机器人控制中心</title>
  <script src="https://cdn.tailwindcss.com"></script>
  <link href="https://cdn.jsdelivr.net/npm/font-awesome@4.7.0/css/font-awesome.min.css" rel="stylesheet" />
  <script>
    tailwind.config = {
      theme: {
        extend: {
          colors: {
            primary: "#3B82F6",
            secondary: "#10B981",
            danger: "#EF4444",
            exploration: "#8B5CF6"
          },
        },
      }
    }
  </script>
  <style type="text/tailwindcss">
    @layer utilities {
      .control-btn {
        @apply w-16 h-16 rounded-full flex items-center justify-center text-white text-xl shadow-lg transition-all duration-200 active:scale-90;
      }
      .control-btn-primary {
        @apply bg-primary hover:bg-blue-600;
      }
      .control-btn-stop {
        @apply bg-danger hover:bg-red-600;
      }
      .confirmation-popup {
        @apply fixed bg-white rounded-lg shadow-xl p-4 z-20 border border-gray-200 transition-all duration-300 transform scale-95 opacity-0;
      }
      .confirmation-popup.show {
        @apply scale-100 opacity-100;
      }
      .debug-panel {
        @apply text-xs bg-gray-50 p-2 rounded border border-gray-200 mt-2;
      }
      .function-btn {
        @apply px-4 py-2 rounded text-sm flex items-center transition-all duration-200 active:scale-95;
      }
      .command-log {
        @apply text-xs bg-gray-50 p-3 rounded border border-gray-200 mt-2 h-32 overflow-y-auto;
      }
      .emergency-btn {
        @apply px-6 py-3 rounded-lg text-base font-bold flex items-center justify-center transition-all duration-200 active:scale-95;
      }
    }
  </style>
</head>
<body class="bg-gray-100 min-h-screen">
  <header class="bg-primary text-white p-4 shadow-md">
    <h1 class="text-2xl font-bold flex items-center">
      <i class="fa fa-robot mr-3"></i>ROS2 机器人控制中心
    </h1>
    <p class="text-blue-100 mt-1">实时监控与远程控制</p>
  </header>

  <main class="container mx-auto p-4">
    <div class="flex flex-col lg:flex-row gap-6">
      <!-- 左侧：栅格地图显示区域 -->
      <div class="lg:w-2/3 bg-white rounded-lg shadow-md p-4">
        <h2 class="text-xl font-semibold mb-3 flex items-center">
          <i class="fa fa-map-o text-primary mr-2"></i>栅格地图
        </h2>
        <div class="relative border border-gray-200 rounded overflow-hidden bg-gray-100 min-h-[400px]">
          <!-- 加载提示 -->
          <div id="map-placeholder" class="text-center text-gray-500 absolute inset-0 flex flex-col items-center justify-center">
            <i class="fa fa-spinner fa-spin text-4xl mb-3"></i>
            <p>等待栅格数据...</p>
            <p id="map-debug" class="text-xs mt-2 text-gray-400">调试: 未收到数据</p>
          </div>

          <!-- 栅格地图Canvas -->
          <canvas id="grid-map-canvas" class="w-full h-auto bg-white hidden"></canvas>

          <!-- 导航确认弹窗 -->
          <div id="navigation-confirmation" class="confirmation-popup">
            <div class="text-sm font-medium mb-2">确认导航到该位置？</div>
            <div class="text-xs text-gray-500 mb-3" id="goal-coordinates">坐标: X=0.00, Y=0.00</div>
            <div class="flex gap-2">
              <button id="confirm-navigation" class="bg-green-500 hover:bg-green-600 text-white px-3 py-1 rounded text-sm flex-1">
                <i class="fa fa-check mr-1"></i> 确认
              </button>
              <button id="cancel-navigation" class="bg-gray-200 hover:bg-gray-300 text-gray-700 px-3 py-1 rounded text-sm flex-1">
                <i class="fa fa-times mr-1"></i> 取消
              </button>
            </div>
          </div>

          <div class="absolute bottom-2 right-2 bg-black/70 text-white text-xs px-2 py-1 rounded">
            <span id="map-status">未连接</span> |
            <span id="position-source">位置源: 未知</span> |
            <span id="robot-coords">X: --, Y: --</span>
          </div>
        </div>

        <!-- 功能控制按钮 -->
        <div class="mt-3 flex flex-wrap gap-3">
          <button id="start-mapping" class="function-btn bg-blue-500 hover:bg-blue-600 text-white">
            <i class="fa fa-map mr-1"></i> 启动建图
          </button>
          <button id="start-navigation" class="function-btn bg-green-500 hover:bg-green-600 text-white">
            <i class="fa fa-compass mr-1"></i> 启动导航
          </button>
          <button id="start-exploration" class="function-btn bg-exploration hover:bg-purple-600 text-white">
            <i class="fa fa-search mr-1"></i> 启动探索
          </button>

          <!-- 新增：紧急停止和恢复按钮 -->
          <button id="emergency-stop" class="function-btn bg-red-600 hover:bg-red-700 text-white">
            <i class="fa fa-exclamation-triangle mr-1"></i> 紧急停止
          </button>
          <button id="resume-control" class="function-btn bg-yellow-500 hover:bg-yellow-600 text-white">
            <i class="fa fa-play-circle mr-1"></i> 恢复控制
          </button>
        </div>

        <!-- 命令执行日志 -->
        <div>
          <div class="text-sm font-medium mt-2 mb-1">命令执行日志:</div>
          <div id="command-log" class="command-log">
            <div class="text-gray-500 italic">等待命令执行...</div>
          </div>
        </div>

        <!-- 地图信息 -->
        <div class="mt-3 flex flex-wrap justify-between items-center gap-2">
          <div class="text-sm text-gray-600">
            <span id="grid-info">栅格尺寸: - × -</span> |
            <span id="resolution-info">分辨率: - 米/栅格</span> |
            <span id="range-info">范围: x[-,-], y[-,-]</span>
          </div>
          <button id="refresh-map" class="text-primary hover:text-blue-700 text-sm flex items-center">
            <i class="fa fa-refresh mr-1"></i> 刷新地图
          </button>
        </div>

        <!-- 数据接收状态调试面板 -->
        <div class="debug-panel">
          <div class="font-medium mb-1">数据接收状态:</div>
          <div class="grid grid-cols-2 gap-x-4 gap-y-1">
            <div><span class="text-gray-500">地图最后更新:</span> <span id="map-last-update">-</span></div>
            <div><span class="text-gray-500">路径最后更新:</span> <span id="path-last-update">-</span></div>
            <div><span class="text-gray-500">TF最后更新:</span> <span id="tf-last-update">-</span></div>
            <div><span class="text-gray-500">位置数据接收:</span> <span id="pose-receive-status">未收到</span></div>
            <div><span class="text-gray-500">紧急状态:</span> <span id="emergency-status">正常</span></div>
          </div>
        </div>
      </div>

      <!-- 右侧：控制区域 -->
      <div class="lg:w-1/3 flex flex-col gap-6">
        <!-- 连接状态 -->
        <div class="bg-white rounded-lg shadow-md p-4">
          <h2 class="text-xl font-semibold mb-3 flex items-center">
            <i class="fa fa-plug text-primary mr-2"></i>连接状态
          </h2>

          <div class="flex items-center mb-4">
            <div id="connection-indicator" class="w-3 h-3 rounded-full bg-red-500 mr-2"></div>
            <span id="connection-status">未连接</span>
          </div>

          <!-- 位置源信息 -->
          <div class="mb-4 p-2 bg-gray-50 rounded border border-gray-200">
            <div class="text-sm font-medium mb-1">位置跟踪</div>
            <div class="flex items-center text-sm">
              <div id="amcl-status" class="w-2 h-2 rounded-full bg-red-500 mr-2"></div>
              <span>AMCL 定位</span>
            </div>
            <div class="flex items-center text-sm mt-1">
              <div id="odom-status" class="w-2 h-2 rounded-full bg-red-500 mr-2"></div>
              <span>里程计 (odom)</span>
            </div>
            <div class="flex items-center text-sm mt-1">
              <div id="tf-status" class="w-2 h-2 rounded-full bg-red-500 mr-2"></div>
              <span>坐标变换 (TF)</span>
            </div>
          </div>

          <!-- TF数据调试 -->
          <div class="mb-4 p-2 bg-gray-50 rounded border border-gray-200">
            <div class="text-sm font-medium mb-1">TF数据 (调试)</div>
            <div class="text-xs text-gray-600 space-y-1" id="tf-debug">
              <div>源框架: -</div>
              <div>目标框架: -</div>
              <div>X: -</div>
              <div>Y: -</div>
              <div>角度: -</div>
            </div>
          </div>

          <!-- 坐标转换调试 -->
          <div class="mb-4 p-2 bg-gray-50 rounded border border-gray-200">
            <div class="text-sm font-medium mb-1">坐标转换 (调试)</div>
            <div class="text-xs text-gray-600 space-y-1" id="coords-debug">
              <div>地图原点: X=-, Y=-</div>
              <div>机器人世界坐标: X=-, Y=-</div>
              <div>机器人栅格坐标: X=-, Y=-</div>
              <div>转换状态: -</div>
            </div>
          </div>

          <!-- 栅格图例 -->
          <div class="border-t border-gray-200 pt-3 mt-3">
            <h3 class="font-medium mb-2">栅格图例</h3>
            <div class="flex flex-wrap gap-4 text-sm">
              <div class="flex items-center">
                <div class="w-4 h-4 bg-white border border-gray-300 mr-2"></div>
                <span>空闲区域 (0)</span>
              </div>
              <div class="flex items-center">
                <div class="w-4 h-4 bg-black mr-2"></div>
                <span>障碍物 (100)</span>
              </div>
              <div class="flex items-center">
                <div class="w-4 h-4 bg-gray-400 mr-2"></div>
                <span>未知区域 (-1)</span>
              </div>
              <div class="flex items-center">
                <div class="w-4 h-4 bg-blue-500 mr-2"></div>
                <span>机器人位置</span>
              </div>
              <div class="flex items-center">
                <div class="w-4 h-4 bg-green-500 mr-2"></div>
                <span>导航路径</span>
              </div>
            </div>
          </div>
        </div>

        <!-- 控制按钮和速度设置 -->
        <div class="bg-white rounded-lg shadow-md p-4">
          <h2 class="text-xl font-semibold mb-4 flex items-center">
            <i class="fa fa-gamepad text-primary mr-2"></i>机器人控制
          </h2>

          <!-- 方向控制按钮 -->
          <div class="grid grid-cols-3 gap-3 mb-6" style="width: 200px; margin: 0 auto;">
            <div class="col-start-2">
              <button id="btn-forward" class="control-btn control-btn-primary">
                <i class="fa fa-arrow-up"></i>
              </button>
            </div>
            <div class="col-start-1">
              <button id="btn-left" class="control-btn control-btn-primary">
                <i class="fa fa-arrow-left"></i>
              </button>
            </div>
            <div class="col-start-2">
              <button id="btn-stop" class="control-btn control-btn-stop">
                <i class="fa fa-stop"></i>
              </button>
            </div>
            <div class="col-start-3">
              <button id="btn-right" class="control-btn control-btn-primary">
                <i class="fa fa-arrow-right"></i>
              </button>
            </div>
            <div class="col-start-2">
              <button id="btn-backward" class="control-btn control-btn-primary">
                <i class="fa fa-arrow-down"></i>
              </button>
            </div>
          </div>

          <!-- 速度控制滑块 -->
          <div class="w-full">
            <div class="mb-4">
              <div class="flex justify-between mb-1">
                <label for="linear-speed" class="text-sm font-medium text-gray-700">线性速度</label>
                <span id="linear-speed-value" class="text-sm text-gray-500">0.5 m/s</span>
              </div>
              <input type="range" id="linear-speed" min="0" max="1" step="0.1" value="0.5"
                    class="w-full h-2 bg-gray-200 rounded-lg appearance-none cursor-pointer accent-primary" />
            </div>
            <div>
              <div class="flex justify-between mb-1">
                <label for="angular-speed" class="text-sm font-medium text-gray-700">角速度</label>
                <span id="angular-speed-value" class="text-sm text-gray-500">1.0 rad/s</span>
              </div>
              <input type="range" id="angular-speed" min="0" max="2" step="0.1" value="1.0"
                    class="w-full h-2 bg-gray-200 rounded-lg appearance-none cursor-pointer accent-primary" />
            </div>
          </div>
        </div>
      </div>
    </div>
  </main>

  <script>
    // 全局变量
    let websocket;
    let mapParameters = null;
    let pendingGoal = null;
    let linearSpeed = 0.5;    // 线性速度默认值
    let angularSpeed = 1.0;   // 角速度默认值
    let currentControlCmd = null;
    let controlInterval = null;
    let robotPose = null;
    let navPath = null;
    let poseHistory = [];     // 用于位置平滑
    const MAX_HISTORY = 5;    // 减少历史记录数量，提高响应速度
    let positionSource = "未知"; // 当前位置数据源
    let tfTransforms = {};    // 存储TF变换数据
    let lastPoseUpdateTime = 0; // 上次位置更新时间戳
    let lastMapUpdateTime = 0; // 上次地图更新时间戳
    let lastPathUpdateTime = 0; // 上次路径更新时间戳
    let lastTfUpdateTime = 0; // 上次TF更新时间戳
    let poseReceived = false; // 是否收到过位置数据
    let emergencyStopActive = false; // 紧急停止状态标志
    let preEmergencyState = { // 保存紧急停止前的状态
      linearSpeed: linearSpeed,
      angularSpeed: angularSpeed,
      controlActive: false,
      navigationActive: false
    };

    // DOM元素引用
    const canvas = document.getElementById('grid-map-canvas');
    const ctx = canvas.getContext('2d');
    const mapPlaceholder = document.getElementById('map-placeholder');
    const mapDebug = document.getElementById('map-debug');
    const robotCoordsDisplay = document.getElementById('robot-coords');
    const tfDebugDisplay = document.getElementById('tf-debug');
    const coordsDebugDisplay = document.getElementById('coords-debug');
    const linearSpeedSlider = document.getElementById("linear-speed");
    const linearSpeedValue = document.getElementById("linear-speed-value");
    const angularSpeedSlider = document.getElementById("angular-speed");
    const angularSpeedValue = document.getElementById("angular-speed-value");
    const mapLastUpdateDisplay = document.getElementById('map-last-update');
    const pathLastUpdateDisplay = document.getElementById('path-last-update');
    const tfLastUpdateDisplay = document.getElementById('tf-last-update');
    const poseReceiveStatusDisplay = document.getElementById('pose-receive-status');
    const commandLog = document.getElementById('command-log');
    const emergencyStatusDisplay = document.getElementById('emergency-status');

    // 初始化WebSocket
    function initWebSocket() {
      const protocol = window.location.protocol === 'https:' ? 'wss:' : 'ws:';
      const url = `${protocol}//${window.location.hostname}:8765`;

      console.log(`[WebSocket] 尝试连接: ${url}`);
      websocket = new WebSocket(url);

      websocket.onopen = () => {
        console.log('[WebSocket] 连接成功');
        updateConnectionStatus(true);
        sendMessage({ type: 'request_data' }); // 主动请求地图数据
        sendMessage({
          type: 'subscribe_topics',
          topics: ['amcl_pose', 'odom', 'tf', 'map', 'path']
        }); // 明确订阅所需话题

        // 定期请求数据，防止数据中断
        setInterval(() => {
          if (websocket.readyState === WebSocket.OPEN) {
            sendMessage({ type: 'request_update' });
            // 定期检查位置数据接收状态
            checkPoseDataStatus();
          }
        }, 1000); // 1秒请求一次
      };

      websocket.onclose = () => {
        console.log('[WebSocket] 连接断开');
        updateConnectionStatus(false);
        mapDebug.textContent = '调试: 连接断开，重试中...';
        logCommand('连接已断开，正在尝试重连...', 'orange');
        setTimeout(initWebSocket, 3000); // 3秒后重试
      };

      websocket.onerror = (err) => {
        console.error('[WebSocket] 错误:', err);
        mapDebug.textContent = `调试: 连接错误: ${err.message}`;
        logCommand(`WebSocket错误: ${err.message}`, 'red');
      };

      websocket.onmessage = (event) => {
        handleServerMessage(event.data);
      };
    }

    // 添加命令日志
    function logCommand(message, color = 'black') {
      const timestamp = new Date().toLocaleTimeString();
      const logEntry = document.createElement('div');
      logEntry.style.color = color;
      logEntry.innerHTML = `<span class="text-gray-400">[${timestamp}]</span> ${message}`;

      // 清空"等待命令执行..."提示
      if (commandLog.querySelector('.italic')) {
        commandLog.innerHTML = '';
      }

      commandLog.appendChild(logEntry);
      commandLog.scrollTop = commandLog.scrollHeight; // 滚动到底部
    }

    // 检查位置数据接收状态
    function checkPoseDataStatus() {
      const now = Date.now();
      // 如果超过5秒未收到位置数据，提示警告
      if (now - lastPoseUpdateTime > 5000) {
        poseReceiveStatusDisplay.innerHTML =
          `<span class="text-red-500">超过${Math.floor((now - lastPoseUpdateTime)/1000)}秒未收到</span>`;
        if (mapParameters) {
          mapDebug.textContent = `调试: 长时间未收到位置数据，请检查服务器是否发送TF/odom数据`;
        }
      } else if (poseReceived) {
        poseReceiveStatusDisplay.innerHTML =
          `<span class="text-green-500">正常 (${Math.floor((now - lastPoseUpdateTime)/1000)}秒前)</span>`;
      }
    }

    // 格式化时间显示
    function formatTime(timestamp) {
      if (!timestamp) return '-';
      const date = new Date(timestamp);
      return `${date.getMinutes()}:${date.getSeconds().toString().padStart(2, '0')}`;
    }

    // 处理服务器消息
    function handleServerMessage(data) {
      try {
        const msg = JSON.parse(data);
        console.log('[收到消息]', msg.type);

        // 更新调试信息
        mapDebug.textContent = `调试: 收到 ${msg.type} 数据`;

        // 处理命令执行状态消息
        if (msg.type === 'command_status') {
          const color = msg.status === 'success' ? 'green' :
                       msg.status === 'error' ? 'red' : 'blue';
          logCommand(`命令执行: ${msg.message}`, color);
          return;
        }

        if (msg.type === 'grid_map' || msg.type === 'map') {
          // 解析栅格数据（字符串转数组）
          const gridData = msg.data_str.split(',').map(Number);

          // 数据校验
          if (gridData.length !== msg.width * msg.height) {
            throw new Error(`数据长度不匹配: 预期 ${msg.width * msg.height}，实际 ${gridData.length}`);
          }

          // 保存地图参数
          mapParameters = {
            width: msg.width,
            height: msg.height,
            resolution: msg.resolution,
            originX: msg.origin_x,
            originY: msg.origin_y,
            data: gridData
          };

          // 更新时间戳
          lastMapUpdateTime = Date.now();
          mapLastUpdateDisplay.textContent = formatTime(lastMapUpdateTime);

          console.log(`[栅格数据] 尺寸: ${msg.width}×${msg.height}, 原点: (${msg.origin_x}, ${msg.origin_y})`);

          // 绘制栅格地图
          drawGridMap();

          // 隐藏加载提示
          mapPlaceholder.classList.add('hidden');
          canvas.classList.remove('hidden');

          // 更新地图信息
          document.getElementById('grid-info').textContent =
            `栅格尺寸: ${msg.width} × ${msg.height}`;
          document.getElementById('resolution-info').textContent =
            `分辨率: ${msg.resolution.toFixed(3)} 米/栅格`;

          // 计算并显示地图范围
          const maxX = msg.origin_x + (msg.width * msg.resolution);
          const maxY = msg.origin_y + (msg.height * msg.resolution);
          document.getElementById('range-info').textContent =
            `范围: x[${msg.origin_x.toFixed(2)}, ${maxX.toFixed(2)}], y[${msg.origin_y.toFixed(2)}, ${maxY.toFixed(2)}]`;

          // 更新坐标调试信息
          updateCoordsDebug({
            mapOrigin: {x: msg.origin_x, y: msg.origin_y}
          });

        } else if (msg.type === 'amcl_pose') {
          // 标记已收到位置数据
          poseReceived = true;
          lastPoseUpdateTime = Date.now();

          // 更新AMCL状态
          document.getElementById('amcl-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';

          // 使用AMCL数据
          updateRobotPose(msg, 'amcl');

        } else if (msg.type === 'odom') {
          // 标记已收到位置数据
          poseReceived = true;
          lastPoseUpdateTime = Date.now();

          // 更新里程计状态
          document.getElementById('odom-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';

          // 使用odom数据
          updateRobotPose(msg, 'odom');

        } else if (msg.type === 'tf') {
          // 更新时间戳
          lastTfUpdateTime = Date.now();
          tfLastUpdateDisplay.textContent = formatTime(lastTfUpdateTime);

          // 更新TF状态
          document.getElementById('tf-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';

          // 存储TF变换数据
          const transformKey = `${msg.header.frame_id}_to_${msg.child_frame_id}`;
          tfTransforms[transformKey] = {
            x: msg.transform.translation.x,
            y: msg.transform.translation.y,
            theta: quaternionToYaw(msg.transform.rotation), // 将四元数转换为偏航角
            timestamp: Date.now()
          };

          // 显示TF调试信息
          updateTFDebug(msg);

          // 处理odom到base_footprint的变换
          if (msg.header.frame_id === 'odom' && msg.child_frame_id === 'base_footprint') {
            // 标记已收到位置数据
            poseReceived = true;
            lastPoseUpdateTime = Date.now();

            // 强制更新里程计状态指示灯
            document.getElementById('odom-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';

            // 使用该TF变换作为机器人位置
            updateRobotPose({
              x: msg.transform.translation.x,
              y: msg.transform.translation.y,
              theta: quaternionToYaw(msg.transform.rotation)
            }, 'tf (odom→base_footprint)');
          }

        } else if (msg.type === 'nav_path' || msg.type === 'path') {
          // 更新时间戳
          lastPathUpdateTime = Date.now();
          pathLastUpdateDisplay.textContent = formatTime(lastPathUpdateTime);

          // 更新导航路径
          navPath = msg.path;
          console.log(`[导航路径] 路径点数量: ${msg.path.length}`);
          logCommand(`收到导航路径，包含 ${msg.path.length} 个路径点`, 'blue');
          drawGridMap(); // 重绘地图以显示新路径
        }
        // 支持robot_pose消息类型
        else if (msg.type === 'robot_pose') {
          poseReceived = true;
          lastPoseUpdateTime = Date.now();

          // 根据消息源更新对应状态灯
          if (msg.source === 'amcl') {
            document.getElementById('amcl-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';
          } else {
            document.getElementById('odom-status').className = 'w-2 h-2 rounded-full bg-green-500 mr-2';
          }

          updateRobotPose(msg, msg.source);
        }
      } catch (err) {
        console.error('[消息处理错误]', err);
        mapDebug.textContent = `调试: 解析错误: ${err.message}`;
        logCommand(`消息处理错误: ${err.message}`, 'red');
      }
    }

    // 更新TF调试信息显示
    function updateTFDebug(tfMsg) {
      const yaw = quaternionToYaw(tfMsg.transform.rotation);
      tfDebugDisplay.innerHTML = `
        <div>源框架: ${tfMsg.header.frame_id}</div>
        <div>目标框架: ${tfMsg.child_frame_id}</div>
        <div>X: ${tfMsg.transform.translation.x.toFixed(4)}</div>
        <div>Y: ${tfMsg.transform.translation.y.toFixed(4)}</div>
        <div>角度: ${yaw.toFixed(4)} rad</div>
      `;
    }

    // 更新坐标转换调试信息
    function updateCoordsDebug(data) {
      let html = '';

      // 地图原点信息
      if (data.mapOrigin) {
        html += `<div>地图原点: X=${data.mapOrigin.x.toFixed(2)}, Y=${data.mapOrigin.y.toFixed(2)}</div>`;
      } else if (mapParameters) {
        html += `<div>地图原点: X=${mapParameters.originX.toFixed(2)}, Y=${mapParameters.originY.toFixed(2)}</div>`;
      } else {
        html += `<div>地图原点: X=-, Y=-</div>`;
      }

      // 机器人世界坐标
      if (data.robotWorldCoords) {
        html += `<div>机器人世界坐标: X=${data.robotWorldCoords.x.toFixed(2)}, Y=${data.robotWorldCoords.y.toFixed(2)}</div>`;
      } else if (robotPose) {
        html += `<div>机器人世界坐标: X=${robotPose.x.toFixed(2)}, Y=${robotPose.y.toFixed(2)}</div>`;
      } else {
        html += `<div>机器人世界坐标: X=-, Y=-</div>`;
      }

      // 机器人栅格坐标
      if (data.robotGridCoords) {
        html += `<div>机器人栅格坐标: X=${data.robotGridCoords.x}, Y=${data.robotGridCoords.y}</div>`;
      } else if (robotPose && mapParameters) {
        const gridX = Math.floor((robotPose.x - mapParameters.originX) / mapParameters.resolution);
        const gridY = Math.floor((robotPose.y - mapParameters.originY) / mapParameters.resolution);
        html += `<div>机器人栅格坐标: X=${gridX}, Y=${gridY}</div>`;
      } else {
        html += `<div>机器人栅格坐标: X=-, Y=-</div>`;
      }

      // 转换状态
      html += `<div>转换状态: ${data.status || '等待数据'}</div>`;

      coordsDebugDisplay.innerHTML = html;
    }

    // 将四元数转换为偏航角（yaw）
    function quaternionToYaw(quat) {
      // 从四元数计算偏航角 (z轴旋转)
      const siny = 2.0 * (quat.w * quat.z + quat.x * quat.y);
      const cosy = 1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z);
      return Math.atan2(siny, cosy);
    }

    // 更新机器人位置
    function updateRobotPose(poseData, source) {
      // 记录更新时间
      lastPoseUpdateTime = Date.now();

      // 更新位置源信息
      positionSource = source;
      document.getElementById('position-source').textContent = `位置源: ${positionSource}`;

      // 更新机器人位置
      const newPose = {
        x: poseData.x,
        y: poseData.y,
        theta: poseData.theta,
        valid: true
      };

      // 添加到历史记录并限制长度
      poseHistory.push(newPose);
      if (poseHistory.length > MAX_HISTORY) {
        poseHistory.shift();
      }

      // 计算平均位置，减少抖动
      let avgX = 0, avgY = 0, avgTheta = 0;
      poseHistory.forEach(pose => {
        avgX += pose.x;
        avgY += pose.y;
        avgTheta += pose.theta;
      });

      robotPose = {
        x: avgX / poseHistory.length,
        y: avgY / poseHistory.length,
        theta: avgTheta / poseHistory.length,
        valid: true
      };

      // 显示机器人坐标
      robotCoordsDisplay.textContent = `X: ${robotPose.x.toFixed(2)}, Y: ${robotPose.y.toFixed(2)}`;

      console.log(`[机器人位置] 源: ${source}, x=${robotPose.x.toFixed(2)}, y=${robotPose.y.toFixed(2)}, θ=${robotPose.theta.toFixed(2)}`);

      // 更新坐标转换调试信息
      if (mapParameters) {
        const gridX = Math.floor((robotPose.x - mapParameters.originX) / mapParameters.resolution);
        const gridY = Math.floor((robotPose.y - mapParameters.originY) / mapParameters.resolution);

        let status = "正常";
        if (gridX < 0 || gridX >= mapParameters.width || gridY < 0 || gridY >= mapParameters.height) {
          status = `<span class="text-red-500">超出地图范围</span>`;
        }

        updateCoordsDebug({
          robotWorldCoords: {x: robotPose.x, y: robotPose.y},
          robotGridCoords: {x: gridX, y: gridY},
          status: status
        });
      }

      drawGridMap(); // 重绘地图以显示新位置
    }

    // 绘制栅格地图
    function drawGridMap() {
      if (!mapParameters) return;

      const { width, height, data, resolution, originX, originY } = mapParameters;

      // 设置Canvas尺寸（自适应容器，保持栅格比例）
      const container = canvas.parentElement;
      const containerWidth = container.clientWidth;
      const containerHeight = container.clientHeight;

      // 计算合适的缩放比例（确保地图完整显示）
      const scaleX = containerWidth / width;
      const scaleY = containerHeight / height;
      const scale = Math.min(scaleX, scaleY, 10); // 最大缩放10倍

      // 仅在尺寸变化时才重新设置Canvas大小
      if (canvas.width !== width * scale || canvas.height !== height * scale) {
        canvas.width = width * scale;
        canvas.height = height * scale;
      }

      // 清空画布
      ctx.clearRect(0, 0, canvas.width, canvas.height);

      // 批量绘制（使用ImageData优化性能）
      const imageData = ctx.createImageData(canvas.width, canvas.height);
      const pixels = imageData.data;

      for (let y = 0; y < height; y++) {
        for (let x = 0; x < width; x++) {
          const index = y * width + x;
          const value = data[index];

          // 计算像素位置
          const pixelX = Math.floor(x * scale);
          const pixelY = Math.floor(y * scale);

          // 根据栅格值设置颜色
          let r, g, b;
          if (value === -1) {
            r = 128; g = 128; b = 128; // 未知区域：灰色
          } else if (value === 0) {
            r = 255; g = 255; b = 255; // 空闲区域：白色
          } else {
            r = 0; g = 0; b = 0; // 障碍物：黑色
          }

          // 填充缩放后的栅格区域
          for (let dy = 0; dy < scale; dy++) {
            for (let dx = 0; dx < scale; dx++) {
              const pos = ((pixelY + dy) * canvas.width + (pixelX + dx)) * 4;
              if (pos < pixels.length) {
                pixels[pos] = r;     // 红色
                pixels[pos + 1] = g; // 绿色
                pixels[pos + 2] = b; // 蓝色
                pixels[pos + 3] = 255; // 不透明度
              }
            }
          }
        }
      }

      // 将像素数据绘制到Canvas
      ctx.putImageData(imageData, 0, 0);

      // 绘制导航路径（如果有）
      if (navPath && navPath.length > 0) {
        ctx.beginPath();
        ctx.strokeStyle = '#10B981'; // 绿色路径
        ctx.lineWidth = 2;

        // 绘制路径点
        for (let i = 0; i < navPath.length; i++) {
          const point = navPath[i];

          // 转换世界坐标到栅格坐标
          const gridX = Math.floor((point.x - originX) / resolution);
          const gridY = Math.floor((point.y - originY) / resolution);

          // 转换到Canvas坐标
          const canvasX = gridX * scale;
          const canvasY = gridY * scale;

          if (i === 0) {
            ctx.moveTo(canvasX, canvasY);
          } else {
            ctx.lineTo(canvasX, canvasY);
          }
        }

        ctx.stroke();
      }

      // 绘制机器人位置（圆形图标）
      if (robotPose && robotPose.valid) {
        // 转换世界坐标到栅格坐标
        const gridX = Math.floor((robotPose.x - originX) / resolution);
        const gridY = Math.floor((robotPose.y - originY) / resolution);

        // 检查是否在地图范围内
        const isInMap = !(gridX < 0 || gridX >= width || gridY < 0 || gridY >= height);

        // 转换到Canvas坐标
        const canvasX = isInMap ? gridX * scale : canvas.width / 2;
        const canvasY = isInMap ? gridY * scale : canvas.height / 2;

        // 绘制机器人
        ctx.save();
        ctx.translate(canvasX, canvasY);
        ctx.rotate(robotPose.theta);

        // 绘制圆形主体
        ctx.beginPath();
        ctx.fillStyle = isInMap ? '#3B82F6' : '#FF9900'; // 正常为蓝色，超出范围为橙色
        ctx.arc(0, 0, 4 * scale, 0, Math.PI * 2); // 半径为4*scale
        ctx.fill();

        // 绘制白色边框
        ctx.beginPath();
        ctx.strokeStyle = '#FFFFFF';
        ctx.lineWidth = 1;
        ctx.arc(0, 0, 4 * scale, 0, Math.PI * 2);
        ctx.stroke();

        // 绘制方向指示线
        ctx.beginPath();
        ctx.strokeStyle = '#FFFFFF';
        ctx.lineWidth = 1.5;
        ctx.moveTo(0, 0);
        ctx.lineTo(0, -6 * scale); // 长度为6*scale
        ctx.stroke();

        // 如果位置超出地图范围，添加提示
        if (!isInMap) {
          ctx.fillStyle = '#FF9900';
          ctx.font = `${12 * scale}px Arial`;
          ctx.textAlign = 'center';
          ctx.fillText('位置超出地图', 0, 30 * scale);
        }

        // 恢复变换
        ctx.restore();

      } else if (mapParameters) {
        // 如果从未收到位置数据，在地图中心显示提示
        ctx.save();
        const centerX = canvas.width / 2;
        const centerY = canvas.height / 2;

        ctx.fillStyle = '#FF0000';
        ctx.font = `${12 * scale}px Arial`;
        ctx.textAlign = 'center';
        ctx.fillText('未收到位置数据', centerX, centerY);
        ctx.fillText('请检查服务器配置', centerX, centerY + 20 * scale);
        ctx.restore();
      }

      // 绘制栅格边框（增强可读性）
      if (scale > 2) {
        ctx.strokeStyle = '#eeeeee';
        ctx.lineWidth = 0.5;
        for (let y = 0; y <= height; y++) {
          ctx.beginPath();
          ctx.moveTo(0, y * scale);
          ctx.lineTo(width * scale, y * scale);
          ctx.stroke();
        }
        for (let x = 0; x <= width; x++) {
          ctx.beginPath();
          ctx.moveTo(x * scale, 0);
          ctx.lineTo(x * scale, height * scale);
          ctx.stroke();
        }
      }
    }

    // 发送消息到服务器
    function sendMessage(msg) {
      if (websocket && websocket.readyState === WebSocket.OPEN) {
        websocket.send(JSON.stringify(msg));
      } else {
        logCommand('无法发送消息：未连接到服务器', 'red');
      }
    }

    // 更新连接状态显示
    function updateConnectionStatus(isConnected) {
      const indicator = document.getElementById('connection-indicator');
      const statusText = document.getElementById('connection-status');
      const mapStatus = document.getElementById('map-status');

      if (isConnected) {
        indicator.className = 'w-3 h-3 rounded-full bg-green-500 mr-2';
        statusText.textContent = '已连接';
        mapStatus.textContent = '已连接';
        logCommand('已连接到服务器', 'green');
      } else {
        indicator.className = 'w-3 h-3 rounded-full bg-red-500 mr-2';
        statusText.textContent = '未连接';
        mapStatus.textContent = '未连接';
      }

      // 定期更新数据状态
      checkPoseDataStatus();
    }

    // 设置控制指令（方向）
    function setControlDirection(direction) {
      // 清除之前的控制指令
      if (controlInterval) {
        clearInterval(controlInterval);
        controlInterval = null;
      }

      switch (direction) {
        case "forward":
          currentControlCmd = () => sendMessage({
            type: "control",
            linear_x: linearSpeed,
            angular_z: 0
          });
          logCommand(`发送前进指令 (速度: ${linearSpeed} m/s)`, 'blue');
          break;
        case "backward":
          currentControlCmd = () => sendMessage({
            type: "control",
            linear_x: -linearSpeed,
            angular_z: 0
          });
          logCommand(`发送后退指令 (速度: ${linearSpeed} m/s)`, 'blue');
          break;
        case "left":
          currentControlCmd = () => sendMessage({
            type: "control",
            linear_x: 0,
            angular_z: angularSpeed
          });
          logCommand(`发送左转指令 (角速度: ${angularSpeed} rad/s)`, 'blue');
          break;
        case "right":
          currentControlCmd = () => sendMessage({
            type: "control",
            linear_x: 0,
            angular_z: -angularSpeed
          });
          logCommand(`发送右转指令 (角速度: ${angularSpeed} rad/s)`, 'blue');
          break;
        case "stop":
          currentControlCmd = null;
          sendMessage({ type: "control", linear_x: 0, angular_z: 0 });
          logCommand('发送停止指令', 'blue');
          return;
      }

      // 启动控制指令发送定时器（200ms一次）
      if (currentControlCmd) {
        controlInterval = setInterval(currentControlCmd, 200);
        currentControlCmd(); // 立即发送一次
      }
    }

    // 紧急停止功能
function activateEmergencyStop() {
      // 保存当前状态
      preEmergencyState = {
        linearSpeed: linearSpeed,
        angularSpeed: angularSpeed,
        controlActive: (controlInterval !== null),
        navigationActive: (pendingGoal !== null)
      };

      // 设置紧急停止状态
      emergencyStopActive = true;

      // 停止所有移动
      setControlDirection("stop");

      // 发送取消导航指令到服务器
      sendMessage({ type: "cancel_navigation" });

      // 取消导航目标（前端）
      if (pendingGoal) {
          document.getElementById('navigation-confirmation').classList.remove('show');
          pendingGoal = null;
          logCommand('已发送紧急停止指令，取消导航并停止机器人', 'red');
      } else {
          logCommand('已发送紧急停止指令，停止所有运动', 'red');
      }

      // 禁用控制按钮
      document.querySelectorAll('.control-btn, .function-btn').forEach(btn => {
        if (btn.id !== 'emergency-stop' && btn.id !== 'resume-control') {
          btn.disabled = true;
        }
      });

      // 禁用速度滑块
      linearSpeedSlider.disabled = true;
      angularSpeedSlider.disabled = true;

      // 更新状态显示
      emergencyStatusDisplay.innerHTML = '<span class="text-red-500 font-bold">已激活</span>';
      logCommand('紧急停止已激活！所有控制已被禁用', 'red');

      // 视觉反馈
      document.body.classList.add('bg-red-50');
      setTimeout(() => {
        document.body.classList.remove('bg-red-50');
      }, 500);
    }

    // 恢复控制功能
    function resumeControl() {
      // 重置紧急停止状态
      emergencyStopActive = false;

      // 启用控制按钮
      document.querySelectorAll('.control-btn, .function-btn').forEach(btn => {
        btn.disabled = false;
      });

      // 启用速度滑块
      linearSpeedSlider.disabled = false;
      angularSpeedSlider.disabled = false;

      // 恢复速度设置
      linearSpeed = preEmergencyState.linearSpeed;
      angularSpeed = preEmergencyState.angularSpeed;
      linearSpeedValue.textContent = `${linearSpeed.toFixed(1)} m/s`;
      angularSpeedValue.textContent = `${angularSpeed.toFixed(1)} rad/s`;

      // 更新状态显示
      emergencyStatusDisplay.textContent = '正常';
      logCommand('已恢复控制，机器人可以正常操作', 'green');

      // 视觉反馈
      document.body.classList.add('bg-green-50');
      setTimeout(() => {
        document.body.classList.remove('bg-green-50');
      }, 500);
    }

    // 初始化控制按钮和事件
    function initControls() {
      // 方向控制按钮
      document.getElementById('btn-forward').addEventListener('click', () =>
        setControlDirection("forward"));
      document.getElementById('btn-backward').addEventListener('click', () =>
        setControlDirection("backward"));
      document.getElementById('btn-left').addEventListener('click', () =>
        setControlDirection("left"));
      document.getElementById('btn-right').addEventListener('click', () =>
        setControlDirection("right"));
      document.getElementById('btn-stop').addEventListener('click', () =>
        setControlDirection("stop"));

      // 速度控制滑块
      linearSpeedSlider.addEventListener("input", (e) => {
        linearSpeed = parseFloat(e.target.value);
        linearSpeedValue.textContent = `${linearSpeed.toFixed(1)} m/s`;
        logCommand(`线性速度调整为: ${linearSpeed.toFixed(1)} m/s`, 'blue');
      });

      angularSpeedSlider.addEventListener("input", (e) => {
        angularSpeed = parseFloat(e.target.value);
        angularSpeedValue.textContent = `${angularSpeed.toFixed(1)} rad/s`;
        logCommand(`角速度调整为: ${angularSpeed.toFixed(1)} rad/s`, 'blue');
      });

      // 刷新地图按钮
      document.getElementById('refresh-map').addEventListener('click', () => {
        logCommand('刷新地图数据请求已发送', 'blue');
        sendMessage({ type: 'request_data' });
        sendMessage({
          type: 'subscribe_topics',
          topics: ['amcl_pose', 'odom', 'tf', 'map', 'path']
        });
      });

      // 导航确认按钮
      document.getElementById('confirm-navigation').addEventListener('click', () => {
        if (pendingGoal) {
          sendMessage({
            type: 'set_goal',
            x: pendingGoal.x,
            y: pendingGoal.y
          });
          logCommand(`设置导航目标: X=${pendingGoal.x.toFixed(2)}, Y=${pendingGoal.y.toFixed(2)}`, 'blue');
          pendingGoal = null;
          document.getElementById('navigation-confirmation').classList.remove('show');
        }
      });

      document.getElementById('cancel-navigation').addEventListener('click', () => {
        pendingGoal = null;
        document.getElementById('navigation-confirmation').classList.remove('show');
        logCommand('导航目标已取消', 'blue');
      });

      // 地图点击事件（导航目标）
      canvas.addEventListener('click', (e) => {
        if (!mapParameters) return;

        const rect = canvas.getBoundingClientRect();
        const scale = canvas.width / mapParameters.width;

        // 计算点击的栅格坐标
        const gridX = Math.floor((e.clientX - rect.left) / scale);
        const gridY = Math.floor((e.clientY - rect.top) / scale);

        // 转换为世界坐标
        const worldX = mapParameters.originX + (gridX * mapParameters.resolution);
        const worldY = mapParameters.originY + (gridY * mapParameters.resolution);

        // 显示确认弹窗
        pendingGoal = { x: worldX, y: worldY };
        document.getElementById('goal-coordinates').textContent =
          `坐标: X=${worldX.toFixed(2)}, Y=${worldY.toFixed(2)}`;

        const popup = document.getElementById('navigation-confirmation');
        popup.style.left = `${e.clientX - rect.left + 20}px`;
        popup.style.top = `${e.clientY - rect.top + 20}px`;
        popup.classList.add('show');
      });

      // 功能控制按钮事件 - 发送多步骤命令
      document.getElementById('start-mapping').addEventListener('click', () => {
        // 建图命令：3个步骤依次执行
        const commands = [
          'bash -c "cd /home/y/fishbot/ && source install/setup.bash && ros2 launch fishbot_description gazebo.launch.py"'
        ];

        logCommand('发送建图命令序列...', 'blue');
        sendMessage({
          type: 'launch_commands',
          command: 'start_mapping',
          steps: commands
        });
      });

      document.getElementById('start-navigation').addEventListener('click', () => {
        // 导航命令示例（根据实际需求修改）
        const commands = [
          'bash -c "cd /home/y/fishbot/ && source install/setup.bash && ros2 launch fishbot_navigation2 navigation2.launch.py "'
        ];

        logCommand('发送导航命令序列...', 'blue');
        sendMessage({
          type: 'launch_commands',
          command: 'start_navigation',
          steps: commands
        });
      });

      document.getElementById('start-exploration').addEventListener('click', () => {
        // 探索命令示例（根据实际需求修改）
        const commands = [
          'bash -c "cd /home/y/fishbot/ && source install/setup.bash && ros2 launch fishbot_cartographer cartographer.launch.py  "'
        ];

        logCommand('发送探索命令序列...', 'blue');
        sendMessage({
          type: 'launch_commands',
          command: 'start_exploration',
          steps: commands
        });
      });

      // 新增：紧急停止按钮事件
      document.getElementById('emergency-stop').addEventListener('click', activateEmergencyStop);

      // 新增：恢复控制按钮事件
      document.getElementById('resume-control').addEventListener('click', resumeControl);
    }

    // 窗口大小变化时重绘地图
    window.addEventListener('resize', drawGridMap);

    // 初始化
    window.addEventListener('load', () => {
      initWebSocket();
      initControls();
    });
  </script>
</body>
</html>